


/* Android app Main activity for Remote Control of an Arduino based car */
/* Full details can be found at yogibotics.blogspot.com */
/*  This program is free software: you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation, either version 3 of the License, or
 (at your option) any later version.
 
 This program is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 GNU General Public License for more details.
 
 You should have received a copy of the GNU General Public License
 along with this program.  If not, see <http://www.gnu.org/licenses/>.
 
 */



package com.yogibotics.remotecontrol;

import java.io.IOException;
import java.util.concurrent.Callable;
import java.util.concurrent.ExecutorService;
import java.util.concurrent.Executors;
import java.util.concurrent.ScheduledExecutorService;
import java.util.concurrent.TimeUnit;

import android.os.Bundle;
import android.os.Handler;
import android.os.Message;
import android.os.PowerManager;
import android.animation.ObjectAnimator;
import android.app.Activity;
import android.bluetooth.BluetoothAdapter;
import android.bluetooth.BluetoothDevice;
import android.content.Context;
import android.content.Intent;
import android.graphics.Color;
import android.hardware.usb.UsbManager;
import android.util.Log;
import android.view.Menu;
import android.view.MotionEvent;
import android.view.View;
import android.view.View.OnTouchListener;
import android.view.animation.LinearInterpolator;
import android.view.animation.RotateAnimation;
import android.widget.ImageView;
import android.widget.TextView;

import com.doromatt.remotecontrol.R;
import com.hoho.android.usbserial.driver.UsbSerialDriver;
import com.hoho.android.usbserial.driver.UsbSerialProber;
import com.hoho.android.usbserial.util.SerialInputOutputManager;

public class MainRemoteActivity extends Activity implements OnTouchListener {

	// Debug switch
	private final boolean debug = true;

	private enum connectionMode {
		USBSERIAL, BTSERIAL // USB or Bluetooth
	};

	private connectionMode currentConnectionMode = connectionMode.USBSERIAL;

	// ==================Comms Protocol Settings===================
	// 3 byte message 'packet' used here.
	// byte 1 - command
	// byte 2, 3 - value (e.g. steering amount)

	// Packet Value setup
	// Look at moving this into an ENUM struncture that's compatible with
	// Arduino C implementation
	private final char COMMAND_THROTTLE = 'a';
	private final char COMMAND_STEERING = 'b';
	private final char COMMAND_STATUS = 'c';
	private final char COMMAND_RPM = 'd';
	private final char COMMAND_CARVOLTAGE = 'e';
	// Comms state Enum
	private final int COMMSTATE_CONNECTED = 1;
	private final int COMMSTATE_DISCONNECTED = 2;

	private int commState = COMMSTATE_DISCONNECTED;
/*	// Known packet error conditions (ENUM shared)

	private final int PACKET_STATUS_VALID_PKT = 1;
	private final int PACKET_STATUS_INCOMPLETE_PKT = 2;
	private final int PACKET_STATUS_BYTE_TIMEOUT = 3;
	private final int PACKET_STATUS_COMMS_LOST = 4;
	private final int PACKET_STATUS_MSG_INVALID = 5;
	private final int PACKET_STATUS_CMD_INVALID = 6;
	private final int PACKET_STATUS_THROTTLE_SIGN_INVALID = 7;
	private final int PACKET_STATUS_THROTTLE_AMNT_INVALID = 8;
	private final int PACKET_STATUS_STEERING_SIGN_INVALID = 9;
	private final int PACKET_STATUS_STEERING_AMNT_INVALID = 10;
*/
	/*
	 ///////////////////
	 // HDLC Packet (Frame) parameters
	 // All data is to be sent as ACSCII characters
	 // Packet Structure
	 //  - start flag
	 //  - command
	 //  - Payload length
	 //  - Payload
	 //  - CRC bytes (length = 1)
	 //  - End Flag
	 //////////////////
	 */
		private final int packetSize = 32; //max size (will never be reached)

		//positions excluding start flag
		private final int PACKET_DETAILS_CMD_LOC = 0;
//		private final int PACKET_LENGTH_LOC = 1;
		private final int PAYLOAD_START_LOC = 2;

		enum PacketState { //Where is the processing of the packet state at
		    PACKETSTART,
		    COMMAND,
		    SIZE,
		    PAYLOAD,
		    CKSUM,      //single byte - not implemented. Set to '0'
		    PACKETEND
		};
		private final char STARTFLAG = '*'; //packet start marker
		private final char ENDFLAG = '#'; //packet end marker

	private	PacketState pktState = PacketState.PACKETSTART;
	// this is the data packet structure.
	// This is where we will keep all the necessary data to keep track of the
	// incoming data.
	private int payloadLength; //payLoad length oc current HDLC packet being processed
	private byte packetData[]; // this is where we will store the data we
								// receive from the serial
	private int curDataLoc = 0; // this is the counter to keep track of how many
								// bytes we've received

	// this tracks the state of the current message
	boolean correctPacket = false;

	// this is used for the timeout
	// the arduino will reset its packet counter if it
	// finds a gap of over set amount between incoming bytes.
//	private final int byteTimeOutVal = 50; // byte timeout value in
											// milliseconds - if bytes are not
											// received within this interval,
											// then discard packet
	private final int pktTimeOut = 450; // threshold between valid packets

	private long lastMsgHit; // last time valid Packet was received
	private long invalidPacketCount; //total number of invalid packets received

	// Heartbeat interval - throttle /steering values sent at this interval
	private int HEARTBEATINTERVAL = 200;

	private final String TAG = MainRemoteActivity.class.getSimpleName();

	// =========RPM Parameters=================

	private final int RPMArraySize = 3;
	private volatile float RPMArray[];
	private volatile int RPMIndex = 0;
	private volatile float RPMTotalCount = 0;
	private final int SpeedoMax = 28; // Max speed on speedometer - to limit
										// movement of needle
	private final float anglePerKm = (float) 9.0; // angle in which speedo needs
													// to move for each km/h
													// reading
	private final int speedoNeedleDelay = 3; // latency in milliseconds between
												// each speedo animation per
												// degree moved
	private float lastSpeedoAngle = 0;

	// =============Telemetry Parameters ================================

	private final float voltageWarningThreshold = (float) 7.9;
	private final float voltageErrorThreshold = (float) 7.4;

	// =============USBController Settings=================================
	/**
	 * The device currently in use, or {@code null}.
	 */
	private UsbSerialDriver mSerialDevice;

	/**
	 * The system's USB service.
	 */
	private UsbManager mUsbManager;

	private SerialInputOutputManager mSerialIoManager;

	private final SerialInputOutputManager.Listener mListener = new SerialInputOutputManager.Listener() {

		@Override
		public void onRunError(Exception e) {
			if (debug)
				Log.d(TAG, "Runner stopped.");
		}

		@Override
		public void onNewData(final byte[] data) {
			MainRemoteActivity.this.runOnUiThread(new Runnable() {
				@Override
				public void run() {
					MainRemoteActivity.this.readHDLCPacket(data);
				}
			});
		}
	};

	// ===========================Bluetooth Parameters==============
	// Message types sent from the BluetoothChatService Handler
	public static final int MESSAGE_STATE_CHANGE = 1;
	public static final int MESSAGE_READ = 2;
	public static final int MESSAGE_WRITE = 3;
	public static final int MESSAGE_DEVICE_NAME = 4;
	public static final int MESSAGE_TOAST = 5;

	// Key names received from the BluetoothChatService Handler
	public static final String DEVICE_NAME = "device_name";
	public static final String BT_DEVICE_ID = "00:12:10:23:05:88";
	public static final String TOAST = "toast";

	// Bluetooth Intent request codes
//	private static final int REQUEST_CONNECT_DEVICE_SECURE = 1;
//	private static final int REQUEST_CONNECT_DEVICE_INSECURE = 2;
	private static final int REQUEST_ENABLE_BT = 3;

	// Name of the connected device
	private String mConnectedDeviceName = null;
	// String buffer for outgoing messages
	// private StringBuffer mOutStringBuffer;
	// Local Bluetooth adapter
	private BluetoothAdapter mBluetoothAdapter = null;
	// Member object for the chat services
	private BluetoothChatService mChatService = null;

	// ======================Background workers=======================
	private final ExecutorService mExecutor = Executors
			.newSingleThreadExecutor();
	private final ExecutorService mExecutorSend = Executors
			.newSingleThreadExecutor(); // Queue for sending messages, seperate
										// from receipt to avoid conflict
	private ScheduledExecutorService mExecutorHeartBeat = Executors
			.newScheduledThreadPool(1); // Timer Task for initiating heartbeat
										// signal
										// (containing throttle and steering
										// parameters)

	// ===================Car Parameters===============================
	private final int wheelDiameter = 90; // wheel diameter in millimeters (for
											// speed calculation)

	// =================User Interface stuff==================================
	// View Coordinates
	private static final float throttlePosnOffset = 20; // distance from end of
														// slider bar to edge of
														// knob
	private static final float steeringRotationMax = 60; // degrees in which
															// steering is
															// allowed to turn

	TextView radioStatusTextView, speedDisplayTextView,
			carBatteryVoltageTextView, carStatusTextView;
	ImageView throttleSliderImageView, steeringImageView, speedoNeedleView;
	// ScrollView statusScrollView;
	float throttleBasetopOffset, throttleBaseHeight, throttleSliderHeight,
			throttleSliderCentreY;
	float lastThrottleTouchY, throttlePercent;
	float steeringWheenAngle, lastSteeringTouchX, lastSteeringTouchY,
			steeringCentreX, steeringCentreY, lastSteeringAngle,
			lastSteeringTouchAngle;
	float steeringOffsetX, steeringOffsetY, steeringPercent,
			lastSteeringPercent;

	// ==============Power Manager to prevent app going to sleep==============

	PowerManager.WakeLock wakeLock;

	@Override
	protected void onCreate(Bundle savedInstanceState) {

		// PowerManager powerManager = (PowerManager)
		// getSystemService(Context.POWER_SERVICE);
		// wakeLock =
		// powerManager.newWakeLock(PowerManager.ACQUIRE_CAUSES_WAKEUP,
		// TAG);
		// wakeLock.acquire();

		super.onCreate(savedInstanceState);
		setContentView(R.layout.activity_main_remote);

		throttleSliderImageView = (ImageView) this
				.findViewById(R.id.imageView_throttleSliderKnob);
		throttleSliderImageView.setOnTouchListener(this);

		radioStatusTextView = (TextView) this
				.findViewById(R.id.textView_radioStatus);
		carStatusTextView = (TextView) this
				.findViewById(R.id.textView_carStatus);
		speedDisplayTextView = (TextView) this
				.findViewById(R.id.textView_Speed);
		carBatteryVoltageTextView = (TextView) this
				.findViewById(R.id.texView_carVoltage);

		// set to Red at start due to no connection
		carBatteryVoltageTextView.setBackgroundColor(Color
				.parseColor("#CC3300"));
		radioStatusTextView.setBackgroundColor(Color.parseColor("#CC3300"));
		carStatusTextView.setBackgroundColor(Color.parseColor("#CC3300"));
		carStatusTextView.setText("No Heartbeat");
		// scrollCellTextView = (TextView) this
		// .findViewById(R.id.textView_scrollText);
		// statusScrollView = (ScrollView) this
		// .findViewById(R.id.scrollView_Status);

		steeringImageView = (ImageView) this
				.findViewById(R.id.imageView_steeringWheel);
		steeringImageView.setOnTouchListener(this);

		steeringWheenAngle = 0;
		lastSteeringAngle = 0;
		lastSteeringTouchAngle = 0;

		speedoNeedleView = (ImageView) this
				.findViewById(R.id.imageView_SpeedoArrow);

		// set up serial connection
		switch (currentConnectionMode) {
		case USBSERIAL:
			// USB stuff
			mUsbManager = (UsbManager) getSystemService(Context.USB_SERVICE);
			break;

		case BTSERIAL:
			// Get local Bluetooth adapter
			mBluetoothAdapter = BluetoothAdapter.getDefaultAdapter();

			// If the adapter is null, then Bluetooth is not supported
			if (mBluetoothAdapter == null) {
				radioStatusTextView.setText("Bluetooth is not available");
				return;
			}
			break;
		}

	}

	// Bluetooth setup
	@Override
	public void onStart() {
		super.onStart();
		if (currentConnectionMode == connectionMode.BTSERIAL) {
			// If BT is not on, request that it be enabled.
			// setupChat() will then be called during onActivityResult
			if (!mBluetoothAdapter.isEnabled()) {
				Intent enableIntent = new Intent(
						BluetoothAdapter.ACTION_REQUEST_ENABLE);
				startActivityForResult(enableIntent, REQUEST_ENABLE_BT);
				// Otherwise, setup the chat session
			} else {
				if (mChatService == null) {
					// Initialize the BluetoothChatService to perform bluetooth
					// connections
					mChatService = new BluetoothChatService(this, mHandler, 1);
					// Get the BluetoothDevice object
					BluetoothDevice device = mBluetoothAdapter
							.getRemoteDevice(BT_DEVICE_ID);
					// Attempt to connect to the device
					mChatService.connect(device, false);

					// Initialize the buffer for outgoing messages
					// mOutStringBuffer = new StringBuffer("");
				}
			}
		}
	}

	public void onWindowFocusChanged(boolean hasFocus) {
		// perform calculation after view has been rendered [ as opposed on
		// OnCreate]
		ImageView throttleBaseImageView = (ImageView) this
				.findViewById(R.id.imageView_throttleSliderBase);

		// set up throttle location / size parameters

		int[] loc = new int[2];
		throttleBaseImageView.getLocationOnScreen(loc);
		throttleBasetopOffset = loc[1];

		throttleBaseHeight = throttleBaseImageView.getDrawable().getBounds()
				.height();

		Log.i(TAG, "Base Offset: " + throttleBasetopOffset);

		throttleSliderHeight = throttleSliderImageView.getDrawable()
				.getBounds().height();
		throttleSliderCentreY = throttleSliderImageView.getY();

		Log.i(TAG, "throttleSliderHeight: " + throttleSliderHeight);
		Log.i(TAG, "throttleBaseHeight " + throttleBaseHeight);
		Log.i(TAG, "throttleBaseImageView.getMeasuredHeight()"
				+ throttleBaseImageView.getMeasuredHeight());
		Log.i(TAG,
				"throttleSliderImageView.getY() "
						+ throttleSliderImageView.getY());

		// set up Steering location / size parameters
		steeringCentreX = steeringImageView.getMeasuredWidth() / 2;
		steeringCentreY = steeringImageView.getMeasuredHeight() / 2;
		steeringImageView.getLocationOnScreen(loc);
		steeringOffsetX = loc[0];
		steeringOffsetY = loc[1];

		Log.i(TAG, "steeringCentreX: " + steeringCentreX);
		Log.i(TAG, "steeringCentreY: " + steeringCentreY);
		Log.i(TAG, "steeringOffsetX: " + steeringOffsetX);
		Log.i(TAG, "steeringOffsetY: " + steeringOffsetY);

	}

	@Override
	public boolean onCreateOptionsMenu(Menu menu) {
		// Inflate the menu; this adds items to the action bar if it is present.
		getMenuInflater().inflate(R.menu.activity_main_remote, menu);
		return true;
	}

	@Override
	protected void onPause() {
		super.onPause();

		// Send a zero throttle message and Stop heartbeat signal
		throttlePercent = 0;
		sendCurrentThrottle();
		shutdownExecutor(mExecutorHeartBeat);

		switch (currentConnectionMode) {
		case USBSERIAL:
			stopIoManager();
			if (mSerialDevice != null) {
				try {
					mSerialDevice.close();
				} catch (IOException e) {
					// Ignore.
				}
				mSerialDevice = null;
			}
			break;
		}

		// if (wakeLock != null)
		// wakeLock.release();

	}

	@Override
	protected void onResume() {
		super.onResume();
		/*
		 * if (wakeLock == null) { PowerManager powerManager = (PowerManager)
		 * getSystemService(Context.POWER_SERVICE); wakeLock =
		 * powerManager.newWakeLock( PowerManager.ACQUIRE_CAUSES_WAKEUP, TAG); }
		 */
		switch (currentConnectionMode) {
		case USBSERIAL:
			mSerialDevice = UsbSerialProber.acquire(mUsbManager);
			if (debug)
				Log.d(TAG, "Resumed, mSerialDevice=" + mSerialDevice);
			if (mSerialDevice == null) {
				radioStatusTextView.setText("No serial device.");
				radioStatusTextView.setBackgroundColor(Color
						.parseColor("#CC3300"));
			} else {
				try {
					mSerialDevice.open();
				} catch (IOException e) {
					Log.e(TAG, "Error setting up device: " + e.getMessage(), e);
					radioStatusTextView.setText("USB Error");
					radioStatusTextView.setBackgroundColor(Color
							.parseColor("#CC3300"));
					try {
						mSerialDevice.close();
					} catch (IOException e2) {
						// Ignore.
					}
					mSerialDevice = null;
					return;
				}
				radioStatusTextView.setText("USB Connected");
				radioStatusTextView.setBackgroundColor(Color
						.parseColor("#7ABA7A"));
			}
			onDeviceStateChange();
			break;
		case BTSERIAL:
			// Performing this check in onResume() covers the case in which BT
			// was
			// not enabled during onStart(), so we were paused to enable it...
			// onResume() will be called when ACTION_REQUEST_ENABLE activity
			// returns.
			if (mChatService != null) {
				// Only if the state is STATE_NONE, do we know that we haven't
				// started already
				if (mChatService.getState() == BluetoothChatService.STATE_NONE) {
					// Start the Bluetooth chat services
					mChatService.start();
				}
			}
			break;
		}

		// Setup heartbeat background task. This will send the throttle value
		// and steering (if changed) on a frequency defined by the heartbeat
		// interval

		final Runnable heartbeat = new Runnable() {
			public void run() {
				// send the throttle value
				sendCurrentThrottle();

				// Send the Steering Value only if it has changed
				if (steeringPercent != lastSteeringPercent) 
					sendCurrentSteering();
				

				// while here check if heartbeat (or any valid) packet has been
				// received during the last window

				if ((System.currentTimeMillis() - pktTimeOut) > lastMsgHit) {
					 
					Log.i(TAG, "Heartbeat timeout - Current:  " +
					 System.currentTimeMillis() + " pktTimeOut: "+ pktTimeOut
					 + " lastMsgHit: " + lastMsgHit);
					 
					Log.i(TAG,  "Invalid Packet Count: " + invalidPacketCount); 
					// update UI on main thread
					if (commState != COMMSTATE_DISCONNECTED) { // don't update
																// if
						// status is already set
						commState = COMMSTATE_DISCONNECTED;
						runOnUiThread(new Runnable() {
							public void run() {
								carStatusTextView.setText("HeartBeat Timeout");
								carStatusTextView.setBackgroundColor(Color
										.parseColor("#CC3300"));

							}
						});
					}
				} else {
					// Log.i(TAG, "Heartbeat ok");
					if (commState != COMMSTATE_CONNECTED) { // don't update if
															// status is already
															// set
						commState = COMMSTATE_CONNECTED;
						// update UI on main thread
						runOnUiThread(new Runnable() {
							public void run() {
//								Log.i(TAG, "Updating hearbeat UI");
								carStatusTextView.setText("HeartBeat OK");
								carStatusTextView.setBackgroundColor(Color
										.parseColor("#7ABA7A"));
							}
						});
					}
				}

			};
		};

		// Cancel all tasks in heartbeat scheduler before inserting new task-
		// just in case there are still some there after a resume from pause
		shutdownExecutor(mExecutorHeartBeat);

		// create a new background worker
		mExecutorHeartBeat = Executors.newScheduledThreadPool(1);
		// insert task into background scheduler
		try {
			mExecutorHeartBeat.scheduleWithFixedDelay(heartbeat, 1000,
					HEARTBEATINTERVAL, TimeUnit.MILLISECONDS);
		} catch (RuntimeException e) {
			Log.e(TAG,
					"Error inserting hearbeat scheuled service: "
							+ e.getMessage() + e.getCause()
							+ e.getLocalizedMessage() + e);
		}
	}

	@Override
	public void onDestroy() {
		super.onDestroy();
		// Stop the Bluetooth chat services
		if (mChatService != null)
			mChatService.stop();
	}

	// method shuts down an ExecutorService in two phases, first by calling
	// shutdown to reject incoming tasks,
	// and then calling shutdownNow, if necessary, to cancel any lingering tasks
	void shutdownExecutor(ExecutorService pool) {
		pool.shutdown(); // Disable new tasks from being submitted
		try {
			// Wait a while for existing tasks to terminate
			if (!pool.awaitTermination(5, TimeUnit.SECONDS)) {
				pool.shutdownNow(); // Cancel currently executing tasks
				// Wait a while for tasks to respond to being cancelled
				if (!pool.awaitTermination(5, TimeUnit.SECONDS))
					Log.e(TAG, "Pool did not terminate");
			}
		} catch (InterruptedException ie) {
			// (Re-)Cancel if current thread also interrupted
			pool.shutdownNow();
			// Preserve interrupt status
			Thread.currentThread().interrupt();
		}
	}

	// USB Connection Specific method
	private void stopIoManager() {
		if (mSerialIoManager != null) {
			Log.i(TAG, "Stopping io manager ..");
			mSerialIoManager.stop();
			mSerialIoManager = null;
		}
	}

	// USB Connection Specific method
	private void startIoManager() {
		if (mSerialDevice != null) {
			Log.i(TAG, "Setting baud rate to 9600..");

			try {
				mSerialDevice.setBaudRate(9600);
			} catch (IOException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
				Log.e(TAG, "Error setting baud rate: " + e.getMessage(), e);
				radioStatusTextView.setText("Error setting baud rate: "
						+ e.getMessage());
			}
			Log.i(TAG, "Starting io manager ..");
			mSerialIoManager = new SerialInputOutputManager(mSerialDevice,
					mListener);
			mExecutor.submit(mSerialIoManager);
		}
	}

	// USB Connection Specific method
	private void onDeviceStateChange() {
		stopIoManager();
		startIoManager();
	}

	// Bluetooth Message Handler
	// The Handler that gets information back from the BluetoothChatService
	private final Handler mHandler = new Handler() {
		@Override
		public void handleMessage(final Message msg) {
			switch (msg.what) {
			case MESSAGE_STATE_CHANGE:
				switch (msg.arg1) {
				case BluetoothChatService.STATE_CONNECTED:
					radioStatusTextView.setText("Bluetooth Connected");
					radioStatusTextView.setBackgroundColor(Color
							.parseColor("#7ABA7A"));
					break;
				case BluetoothChatService.STATE_CONNECTING:
					radioStatusTextView.setText("Bluetooth Connecting");
					radioStatusTextView.setBackgroundColor(Color
							.parseColor("#FF9900"));

					break;
				case BluetoothChatService.STATE_LISTEN:
				case BluetoothChatService.STATE_NONE:
					radioStatusTextView.setText("Bluetooth Error");
					radioStatusTextView.setBackgroundColor(Color
							.parseColor("#CC3300"));

					break;
				}
				break;
			case MESSAGE_WRITE:
				// byte[] writeBuf = (byte[]) msg.obj;
				// construct a string from the buffer
				// String writeMessage = new String(writeBuf);
				// mConversationArrayAdapter.add("Me:  " + writeMessage);
				break;
			case MESSAGE_READ:
				byte[] readBuf = (byte[]) msg.obj;
				// check is prev byte has completed
				readHDLCPacket(readBuf);

				break;
			case MESSAGE_DEVICE_NAME:
				// save the connected device's name
				mConnectedDeviceName = msg.getData().getString(DEVICE_NAME);
				radioStatusTextView.setText("Connected to BT Device: "
						+ mConnectedDeviceName);

				break;
			// case MESSAGE_TOAST:
			// Toast.makeText(getApplicationContext(),
			// msg.getData().getString(TOAST),
			// Toast.LENGTH_SHORT).show();
			// break;
			}
		}
	};

	public void sendMessage(final String msg) {

//	    Log.i(TAG, "Sending msg: " + msg);
		switch (currentConnectionMode) {
		case USBSERIAL:
			if (mSerialDevice != null) {
				Callable<Object> c = new Callable<Object>() {
					public Object call() throws Exception {

						int bytesSent = 0;

						byte[] src = msg.getBytes();
						// try {
						bytesSent = mSerialDevice.write(src, 50);
						/*
						 * } catch (IOException e) { Log.e(TAG,
						 * "Error Sending Message: " + e.getMessage(), e);
						 * mTitleTextView.setText("Error Sending Message: " +
						 * e.getMessage()); try { mSerialDevice.close(); } catch
						 * (IOException e2) { // Ignore. } mSerialDevice = null;
						 * return bytesSent; }
						 */return bytesSent;
					}

				};

				// Future future = mExecutorSend.submit(c);
				mExecutorSend.submit(c);
				// Log.i(TAG, "Send Message - Executor task submitted ");
				/*
				 * int k =0; try { k = (Integer) future.get(); } catch
				 * (Exception e) { e.printStackTrace(); }
				 * stausTextView.setText("Written bytes : " + k);
				 */
			}
			break;

		case BTSERIAL:
			// Check that we're actually connected before trying anything
			if (mChatService.getState() == BluetoothChatService.STATE_CONNECTED) {

				// Check that there's actually something to send
				if (msg.length() > 0) {

					Callable<Object> cBT = new Callable<Object>() {
						public Object call() throws Exception {

							// Get the message bytes and tell the
							// BluetoothChatService
							// to write
							byte[] send = msg.getBytes();
							mChatService.write(send);

							return 0;
						}
					};
					// submit as a background task
					mExecutorSend.submit(cBT);
					// Reset out string buffer to zero and clear the
					// edit text
					// field ----Not sure of this is still required
					// mOutStringBuffer.setLength(0);
				}
			}
			break;
		}
	}


	private void processPacketData(byte buff[], int dataLength){
	// This method interprets the payLoad data received from Arduino and updates the Android device with telemetry info
		

/*				if (debug)	Log.d(TAG,
							" Packet: "
									+ (char) packetData[PACKET_DETAILS_CMD_LOC]
									+ twoBytesToInt(
											packetData[PACKET_DETAILS_ITEM_1],
											packetData[PACKET_DETAILS_ITEM_2]));
				/*
				 * if (debug) Log.d(TAG, "Command char in packet: " + (char)
				 * packetData[PACKET_DETAILS_CMD_LOC] + " ASCII Value: " + (int)
				 * (char) packetData[PACKET_DETAILS_CMD_LOC]);

				 */
						// check the first byte in the packet
		// Check if it is a valid command message

				switch (buff[PACKET_DETAILS_CMD_LOC]) {

				// Handle Throttle Command
				case COMMAND_STATUS:

					// insert status interpretations here

					break;

				// Handle RPM metric
				case COMMAND_RPM:
//					if (debug) Log.d(TAG, "RPM packet received");
					String rpmData = "";
					
					for (int q = PAYLOAD_START_LOC; q < (PAYLOAD_START_LOC+ dataLength); q++)
						rpmData = rpmData + ((char)buff[q]);
					int rpm10 = Integer.parseInt(rpmData);

//					 if (debug) Log.d(TAG, "RPM10: " + rpm10);

					float rpm = (float) ((float) rpm10 / 10.0); // divide by 10
																// here as
																// Arduino had
																// multiplid by
																// 10 so 1
																// decimal point
																// could be sent

					// Smoothing function
					if (RPMArray == null)
						RPMArray = new float[RPMArraySize];

					RPMTotalCount = RPMTotalCount - RPMArray[RPMIndex] + rpm;
					RPMArray[RPMIndex] = rpm;

					RPMIndex++; // advance the index
					if (RPMIndex > (RPMArraySize - 1))
						RPMIndex = 0; // rotate back to beginning

					float averageRPM = RPMTotalCount / RPMArraySize;
					// if (debug) Log.d(TAG, "RPMTotalCount: " + RPMTotalCount +
					// " Average RPM: " + averageRPM);
					// calculate speed in km/h

					updateSpeed(averageRPM);

					break;

				case COMMAND_CARVOLTAGE:
//					if (debug) Log.d(TAG, "Voltage packet received");
					String volatageData = "";
					
					for (int q = PAYLOAD_START_LOC; q < (PAYLOAD_START_LOC+ dataLength); q++)
						volatageData = volatageData + ((char)buff[q]);
					int volts100 = Integer.parseInt(volatageData);
					
					// divide by 100 as it was multiplied prior to transition
					// and
					// convert to float
					float carVoltage = (float) ((float) volts100 / 100.0);

					carBatteryVoltageTextView.setText(String.format("%.2f%n",
							carVoltage) + "V");

					if (carVoltage < voltageErrorThreshold) { // error threshold
						carBatteryVoltageTextView.setBackgroundColor(Color
								.parseColor("#CC3300"));
					} else if (carVoltage > voltageWarningThreshold) { // voltage
																		// okay
						carBatteryVoltageTextView.setBackgroundColor(Color
								.parseColor("#7ABA7A"));
					} else { // warning area
						carBatteryVoltageTextView.setBackgroundColor(Color
								.parseColor("#FF9900"));
					}

					break;

				default:

					// if here, the command byte is wrong
					// so dump the current dataPacket

					invalidPacketCount++;
					if (debug)
						Log.d(TAG, "Invalid Command");
					break;
				}

			}
		


	public void updateSpeed(float avgRPM) {

		float speed = (float) ((float) (avgRPM * wheelDiameter * Math.PI * 60.0) / (1000.0 * 1000.0));

		speedDisplayTextView.setText(String.format("%.1f%n", speed) + " km/h");

		// limit needle movement within analog dial
		float endAngle;
		if (speed < SpeedoMax)
			endAngle = speed * anglePerKm;
		else
			endAngle = SpeedoMax * anglePerKm;


		RotateAnimation r = new RotateAnimation(lastSpeedoAngle, endAngle,
				speedoNeedleView.getDrawable().getBounds().width() / 2,
				speedoNeedleView.getDrawable().getBounds().height() / 2); // HERE
		r.setStartOffset(0);
		r.setDuration(speedoNeedleDelay * (long) Math.abs(lastSpeedoAngle - endAngle));
		r.setFillAfter(true);
		r.setFillEnabled(true);

		speedoNeedleView.startAnimation(r);

		lastSpeedoAngle = endAngle;

	}
	
	
	
	
	
	
///////////////////////////////////////////////////////////
//
// Touch Event Handlers
//
///////////////////////////////////////
	
	// handle Touch Events
	public boolean onTouch(View v, MotionEvent event) {

		if (v == throttleSliderImageView) {
			handleThrottleTouch(event);
		}

		if (v == steeringImageView) {
			handleSteeringTouch(event);
		}
		return true;

	}

	public void handleThrottleTouch(MotionEvent event) {

		int action = event.getAction();
		switch (action) {
		case MotionEvent.ACTION_DOWN:
			lastThrottleTouchY = event.getRawY();
			break;
		case MotionEvent.ACTION_MOVE:

			float Y = event.getRawY();

			float deltaY = Y - lastThrottleTouchY;

			float newPosnY = throttleSliderImageView.getY() + deltaY;
			/*
			 * if (debug) Log.i(TAG, "Current Y: " +
			 * throttleSliderImageView.getY()); if (debug) Log.i(TAG,
			 * "touch event Y: " + Y); Log.i(TAG, "New Y Posn: " + newPosnY);
			 * Log.i(TAG,
			 * "(throttleBaseHeight - throttlePosnOffset- throttleSliderHeight/2) "
			 * + (throttleBaseHeight - throttlePosnOffset -
			 * throttleSliderHeight/ 2));
			 */
			// do not move beyond edges
			if ((newPosnY < (throttleBaseHeight - throttlePosnOffset - throttleSliderHeight / 2) && (newPosnY > throttlePosnOffset)))
				throttleSliderImageView.setY(newPosnY);

			lastThrottleTouchY = Y;
			// String throttleMsg;
			// calculate % throttle applied to send to Serial Device - Need to
			// improve calculation here
			if (newPosnY <= throttleSliderCentreY) { // Forward throttle
				throttlePercent = (throttleSliderCentreY - newPosnY)
						/ (throttleSliderCentreY - throttlePosnOffset) * 100;
				// constrain values
				if (throttlePercent > 100)
					throttlePercent = 100;

			} else { // reverse throttle
				/*
				 * throttlePercent = -1 (newPosnY - throttleSliderCentreY) /
				 * (throttleBaseHeight - (throttleSliderHeight / 2) -
				 * throttleSliderCentreY) 100;
				 */
				throttlePercent = -1
						* (newPosnY - throttleSliderCentreY)
						/ (throttleBaseHeight - throttleSliderHeight
								- throttleSliderCentreY - throttlePosnOffset)
						* 100;
				// constrain values
				if (throttlePercent < -100)
					throttlePercent = -100;

			}

			// No need to send message here as it'll be done by the heartbeat
			// process

			// Log.i(TAG, "throttlePercent: " + throttlePercent);

			break;

		case MotionEvent.ACTION_UP:
			/*
			 * TranslateAnimation anim = new TranslateAnimation
			 * (throttleSliderImageView.getX(), throttleSliderImageView.getX(),
			 * throttleSliderImageView.getY(), throttleBaseHeight/2);
			 * anim.setDuration(300);
			 * throttleSliderImageView.startAnimation(anim);
			 */
			// return slider to centre when touch ended
			ObjectAnimator transAnimation = ObjectAnimator.ofFloat(
					throttleSliderImageView, "Y",
					throttleSliderImageView.getY(), throttleSliderCentreY);
			transAnimation.setDuration(300);
			transAnimation.start();

			// set throttle to zero
			throttlePercent = 0;
			sendCurrentThrottle();
			break;
		case MotionEvent.ACTION_CANCEL:
			break;
		default:
			break;
		}

	}

	public void handleSteeringTouch(MotionEvent event) {
		float X, Y = 0;

		int action = event.getAction();
		switch (action) {
		case MotionEvent.ACTION_DOWN:
			lastSteeringTouchX = event.getRawX();
			lastSteeringTouchY = event.getRawY();

			X = event.getRawX() - steeringOffsetX;
			Y = event.getRawY() - steeringOffsetY;

			lastSteeringTouchAngle = getTheta(X, Y);
			// Log.i(TAG, "Initial Touch - lastSteeringTouchAngle: "
			// + lastSteeringTouchAngle);

			break;
		case MotionEvent.ACTION_MOVE:

			// X and Y coordinates of touch relative to view
			X = event.getRawX() - steeringOffsetX;
			Y = event.getRawY() - steeringOffsetY;

//			 Log.i(TAG,"Steering Touch X: " + X);
//			 Log.i(TAG,"Steering Touch Y: " + Y);

			// calculate angle of touch movement relative to wheel centre
			// (calculate function knows about centre)
			float currentSteeringTouchAngle = getTheta(X, Y);
//			 Log.i(TAG, "currentSteeringTouchAngle: "
//			 + currentSteeringTouchAngle);

			float deltaSteeringTouchAngle = currentSteeringTouchAngle
					- lastSteeringTouchAngle;

			// adjust for anomalies caused by turning over 360 degrees (if
			// changes are over 300 degrees (nominal) then assume full turn has
			// occured)
			// /there must be a more elegant solution here!

			if (deltaSteeringTouchAngle > 300)
				deltaSteeringTouchAngle = deltaSteeringTouchAngle - 360;

			if (deltaSteeringTouchAngle < -300)
				deltaSteeringTouchAngle = deltaSteeringTouchAngle + 360;

//			 Log.i(TAG, "deltaSteeringTouchAngle: " +
//			 deltaSteeringTouchAngle);

			// wheel angle = rotation from initial position, touch angle - angle
			// of change in touch (can be anywhere in wheel)
			// what is then the actual angle of the wheel turn

			float currentSteeringWheelAngle = lastSteeringAngle
					+ deltaSteeringTouchAngle;
//			 Log.i(TAG, "currentSteeringWheelAngle: " +
//			 currentSteeringWheelAngle);

			lastSteeringTouchAngle = currentSteeringTouchAngle;

			// restrict movement- should make this more efficient - no need to
			// perform rotation if at extremes

			if (currentSteeringWheelAngle < -1 * steeringRotationMax)
				currentSteeringWheelAngle = -1 * steeringRotationMax;

			if (currentSteeringWheelAngle > steeringRotationMax)
				currentSteeringWheelAngle = steeringRotationMax;
/*
			Matrix matrix = new Matrix();
			steeringImageView.setScaleType(ScaleType.MATRIX); // required
			matrix.postRotate(currentSteeringWheelAngle, steeringCentreX,
					steeringCentreY);
			steeringImageView.setImageMatrix(matrix);
*/
			RotateAnimation animSteering = new RotateAnimation(lastSteeringAngle, currentSteeringWheelAngle,
					 steeringCentreX, steeringCentreY);
			animSteering.setInterpolator(new LinearInterpolator());
			animSteering.setRepeatCount(0);
			animSteering.setFillAfter(true);
			animSteering.setFillEnabled(true);
			animSteering.setDuration(10);

			steeringImageView.startAnimation(animSteering);
			
			lastSteeringTouchX = X;
			lastSteeringTouchY = Y;

			// reset the 'last' steering angle
			lastSteeringAngle = currentSteeringWheelAngle;

			// calculate steeringPercent
			steeringPercent = currentSteeringWheelAngle / steeringRotationMax
					* 100;

			//time to send the packet
			// String steeringMsg;
			if (steeringPercent >= 0) {// right turn
				// constrain values
				if (steeringPercent > 100)
					steeringPercent = 100;
				// steeringMsg = STEERING + "+" + (char) (int) steeringPercent;
			} else {// left turn
				if (steeringPercent < -100)
					steeringPercent = -100;
				// steeringMsg = STEERING + "-"
				// + (char) (int) (-1 * steeringPercent);
			}

			// Do not need to send steering message here as it'll be handled by
			// the heartbeat worker

			break;

		case MotionEvent.ACTION_UP:

			// Log.i(TAG, "Action Up - lastSteeringAngle: " +
			// lastSteeringAngle);
			//move steering back to centre
			RotateAnimation anim = new RotateAnimation(lastSteeringAngle, 0, steeringCentreX, steeringCentreY);
			anim.setInterpolator(new LinearInterpolator());
			
			
			anim.setRepeatCount(0);
			anim.setFillAfter(true);
			anim.setFillEnabled(true);
			anim.setDuration(200);

			steeringImageView.startAnimation(anim);
			lastSteeringAngle = 0;
			steeringPercent = 0;
			sendCurrentSteering();

			break;

		case MotionEvent.ACTION_CANCEL:

			break;
		default:
			break;
		}

	}

	private float getTheta(float x, float y) {
		float sx = x - steeringCentreX;
		float sy = y - steeringCentreY;

		float length = (float) Math.sqrt(sx * sx + sy * sy);
		float nx = sx / length;
		float ny = sy / length;
		float theta = (float) Math.atan2(ny, nx);

		final float rad2deg = (float) (180.0 / Math.PI);
		float theta2 = theta * rad2deg;

		return (theta2 < 0) ? theta2 + 360.0f : theta2;
	}


////////////////////////////////////////////////////////////
//
//	HDLC Protocol Methods
//
//////////////////////////////////////////////////////////

	private String getHDLCPacket(char cmd, String payLoad){
		
		String HDLCPacket = "*" 
		+ cmd 
		+ payLoad.length() 
		+ payLoad 
		+ (int)calcCRC(payLoad)
		+ "#";
		
//		Log.i(TAG, "HDLC Packet: " + HDLCPacket);
		
		return HDLCPacket;
	}
	
	private int calcCRC(String payLoad){
		//not implemented yet, always set to 0
		return 0;
	}
	
	// Handle data events
	// USB and BT events handled here
	private void readHDLCPacket(final byte[] serialData) {
		//This function reads the serial buffer and constructs the HDLC packet. Once a valid packet is found it dispatches it for processing.
		//Any invalid packets are discarded.
		
//		 if (debug) Log.d(TAG, "Byte Received. Length: " + serialData.length + " data: " + serialData);

		if (packetData==null)
			packetData = new byte[packetSize];
		
		correctPacket = false;
		for (int i = 0; i < serialData.length; i++) {

			
			
			
	        byte byteIn = serialData[i];
	        //    Serial.print("Read in: ");
	        //   Log.i(TAG,byteIn);
	        if (byteIn == STARTFLAG){
	            if (pktState != PacketState.PACKETSTART){
	                Log.i(TAG,"Discarded Last Packet");
	                invalidPacketCount++;
	            }
	            pktState = PacketState.COMMAND; //reset packet and get ready to read next byte
	            curDataLoc = 0;
	            //      Log.i(TAG,"Starting Packet....");
	            
	        }
	        else{
	            //Log.i(TAG,dataPacket.data[dataPacket.curLoc]);
	            //     Serial.print("dataPacket.curLoc: ");
	            //     Log.i(TAG,dataPacket.curLoc);
	        	packetData[curDataLoc] = byteIn;
	            
	            
	            switch (pktState){
	                case COMMAND:
	                    //        Serial.print("Command: ");
	                    //        Log.i(TAG,byteIn);
	                    pktState = PacketState.SIZE; // next byte should be
	                    break;
	                    
	                case SIZE:
	                    payloadLength = byteIn - '0';
	                    if (payloadLength > 0)
	                        pktState = PacketState.PAYLOAD;
	                    else
	                        pktState = PacketState.CKSUM;
	                    
	                    //        Serial.print("Payload Size: ");
	                    //        Log.i(TAG,payloadLength);
	                    break;
	                    
	                case PAYLOAD:
	                    if (curDataLoc == (payloadLength + 1)) {
	                        pktState = PacketState.CKSUM;
	                    }
	                    //        Serial.print("Payload byte: ");
	                    //        Log.i(TAG,byteIn);
	                    break;
	                    
	                case CKSUM:
	                    //    if (calc_CRC(dataPacket.data) == (byteIn = '0')) {
	                    //        Serial.print("CKSUM: ");
	                    //        Log.i(TAG,byteIn);
	                    
	                    //        Serial.print("calc_CRC(): ");
	                    //        Log.i(TAG,calc_CRC());
	                    //        Serial.print("(byte)'0': ");
	                    //        Log.i(TAG,(byte)'0');
	                    if (calcCRC("") == (byteIn - (byte)'0')) { //calcCRC always returns 0 for now
	                        
	                        correctPacket = true;
	                        pktState = PacketState.PACKETEND;
	                        //          Log.i(TAG,"Checksum OK");
	                        
	                    }
	                    else{
	                        pktState = PacketState.PACKETSTART; //discard packet
	                        curDataLoc =0;
	                        //          Log.i(TAG,"Invalid Checksum ");
	                        invalidPacketCount++;
	                    }
	                    
	                    
	                    break;
	                    
	                case PACKETEND:
	                    //        Serial.print("Packet End: ");
	                    //        Log.i(TAG,byteIn);
	                    //        Serial.print("ENDFLAG: ");
	                    //        Log.i(TAG,ENDFLAG);
	                    //       Serial.print("correctPacket: ");
	                    //        Log.i(TAG,correctPacket);
	                    if ((byteIn == ENDFLAG) && correctPacket) {
	                        
	 /*                       
	                        Serial.print("Correct Message Received: ");
	                        for (int i=0; i<=dataPacket.curLoc;i++)
	                            Serial.print(char(dataPacket.data[i]));
	                        Serial.print("  At Time: ");
	                        Log.i(TAG,millis());
	                        
	                        
	  */                      
	                    	processPacketData(packetData, payloadLength);
	                        lastMsgHit = System.currentTimeMillis();
	                        
	                        
	                        pktState = PacketState.PACKETSTART;
	                        curDataLoc = 0;
	                        
	                        
	                        
	                        
	                    }
	                    else{ //discard packet as packet terminator is not in expected position
	                        pktState = PacketState.PACKETSTART;
	                        curDataLoc = 0;
	                        correctPacket = false;
	                        invalidPacketCount++;
	                    }
	                    break;
	                    
	            }
	            
	            curDataLoc++;
	        }

		}
	}
	

	

	private void sendCurrentThrottle(){
		
		String throttlePayload;
		if (throttlePercent >= 0) { // Forward throttle
			throttlePayload = "+" + (int) throttlePercent;
		} else { // reverse throttle
			throttlePayload = "-" + (int) (-1 * throttlePercent); // convertion
															// to
															// positive
															// int
															// and
															// calcualte
															// ASCII
															// value
		}

		String pckt = getHDLCPacket(COMMAND_THROTTLE, throttlePayload);
		sendMessage(pckt);
		
	}
	
	private void sendCurrentSteering(){
		
		String steeringPayLoad;
		if (steeringPercent >= 0) {// right turn
			// constrain values
			if (steeringPercent > 100)
				steeringPercent = 100;
			steeringPayLoad = "+"
					+ (int) steeringPercent;
		} else {// left turn
			if (steeringPercent < -100)
				steeringPercent = -100;
			steeringPayLoad = "-"
					+ (int) (-1 * steeringPercent);
		}

		// send steering message
		String pckt = getHDLCPacket(COMMAND_STEERING, steeringPayLoad);
		sendMessage(pckt);
		lastSteeringPercent = steeringPercent;
	}
}


